#include <assert.h>
#include <scene/skeleton.h>
#include <scene/skeletonbone.h>

ceSkeleton::ceSkeleton()
  : _matrices(0)
	, _neutralMatrices(0)
	, _rootBone (0)
{
	CE_OBJECT_CONSTR;
  _bones.clear();
}


void ceSkeleton::AddBone(ceSkeletonBone *bone, ceSkeletonBone* parent)
{
  assert (bone);

  _bones.push_back(bone);
  bone->AddRef();

  if (parent)
		{
			parent->AddBone (bone);
		}
	else
    {
      CE_SET(_rootBone, bone);
    }
}

void ceSkeleton::RemoveBone(ceSkeletonBone *bone)
{
  if (_rootBone == bone)
    {
      CE_UNSET(_rootBone);
    }
  else if (_rootBone)
    {
      _rootBone->RemoveBone(bone, true);
    }

	std::vector<ceSkeletonBone*>::iterator it, eit;
	for (it = _bones.begin (), eit=_bones.end() ; it != eit; ++it)
    {
      ceSkeletonBone* b = *it;
      if (b == bone)
        {
          _bones.erase(it);
          b->Release();
          break;
        }
    }
}

void ceSkeleton::SetRootBone(ceSkeletonBone *bone)
{
  CE_SET (_rootBone, bone);
}

void ceSkeleton::Initialize(const ceMatrix4f& matrix)
{
	assert (_rootBone);

	if (_matrices)
		{
			delete [] _matrices;
		}
	if (_neutralMatrices)
		{
			delete [] _neutralMatrices;
		}

	// create the array for the bone matrices
	_matrices = new ceMatrix4f[_bones.size()];
	_neutralMatrices = new ceMatrix4f[_bones.size ()];

	// now iterate over all bones and give them the right indes
	std::list<ceSkeletonBone*>::iterator bit;
	for (unsigned i=0, ni=_bones.size(); i<ni; ++i)
		{
			_bones[i]->_idx = i;
		}

	_rootBone->UpdateBoneTransformation (matrix, _neutralMatrices);

	for (unsigned i=0, ni=_bones.size (); i<ni; ++i)
		{
			_neutralMatrices[i].FastInvert ();
		}
}

void ceSkeleton::Update (const ceMatrix4f& matrix)
{
	assert (_rootBone);

	_rootBone->UpdateBoneTransformation (matrix, _matrices);

	for (unsigned i=0, ni=_bones.size (); i<ni; ++i)
		{
			_matrices[i] = _matrices[i] * _neutralMatrices [i];
		}

}


unsigned ceSkeleton::GetNumberOfBones () const
{
	return _bones.size ();
}

const ceMatrix4f* ceSkeleton::GetBoneMatrices () const
{
	return _matrices;
}

ceSkeletonBone* ceSkeleton::GetBone (unsigned idx)
{
	for (unsigned i=0, ni=_bones.size(); i<ni; ++i)
		{
			if (i == idx)
				{
					return _bones[i];
				}
		}
	return 0;
}


const ceSkeletonBone* ceSkeleton::GetBone (unsigned idx) const
{
	for (unsigned i=0, ni=_bones.size(); i<ni; ++i)
		{
			if (i == idx)
				{
					return _bones[i];
				}
		}
	return 0;
}


ceSkeletonBone* ceSkeleton::GetBone (const std::string& name)
{
	for (unsigned i=0, ni=_bones.size(); i<ni; ++i)
		{
			ceSkeletonBone* bone = _bones[i];
			if (bone->GetName () == name)
				{
					return bone;
				}
		}
	return 0;
}

const ceSkeletonBone* ceSkeleton::GetBone (const std::string& name) const
{
	for (unsigned i=0, ni=_bones.size(); i<ni; ++i)
		{
			ceSkeletonBone* bone = _bones[i];
			if (bone->GetName () == name)
				{
					return bone;
				}
		}
	return 0;
}

